#include <bsl_err_internal.h>
#include <bsl_errno.h>
#include <bsl_params.h>
#include <bsl_sal.h>
#include <crypt_algid.h>
#include <crypt_errno.h>
#include <crypt_types.h>
#include <crypt_eal_cipher.h>
#include <securec.h>

#include <wd.h>
#include <wd_bmm.h>
#include <wd_cipher.h>

#include <stdlib.h>

#include "kaep_err.h"
#include "kaev1_modes.h"
#include "kaev1_memory.h"

typedef uint64_t Gf128[2];

typedef enum {
    NO_BLK_LEFT,
    BLK_LEFT,
} KAE_Modes_UpdateType;

KaeQueuePoolHead *gModesNodePool = NULL;

int WdModesInitQnodePool(void)
{
    KaeQueuePoolDestroy(gModesNodePool);

    gModesNodePool = KaeInitQueuePool(WCRYPTO_CIPHER);
    if (gModesNodePool == NULL) {
        return KAE_WD_DO_DIGEST_FAIL;
    }
    return CRYPT_SUCCESS;
}

int WdModesDestroyQnodePool(void)
{
    KaeQueuePoolDestroy(gModesNodePool);
    return CRYPT_SUCCESS;
}

static uint64_t ReverseBits(uint64_t a)
{
    uint64_t r = 0;
    int i;

    for (i = 0; i < 63; i++) {
        r |= a & 1;
        r <<= 1;
        a >>= 1;
    }
    r |= a & 1;
    return r;
}

#define GETU64(p)                                                                                        \
    ((uint64_t)(p)[0] << 56 | (uint64_t)(p)[1] << 48 | (uint64_t)(p)[2] << 40 | (uint64_t)(p)[3] << 32 | \
     (uint64_t)(p)[4] << 24 | (uint64_t)(p)[5] << 16 | (uint64_t)(p)[6] << 8 | (uint64_t)(p)[7])

#define PUTU64(p, V)                                                                              \
    ((p)[0] = (uint8_t)((V) >> 56), (p)[1] = (uint8_t)((V) >> 48), (p)[2] = (uint8_t)((V) >> 40), \
     (p)[3] = (uint8_t)((V) >> 32), (p)[4] = (uint8_t)((V) >> 24), (p)[5] = (uint8_t)((V) >> 16), \
     (p)[6] = (uint8_t)((V) >> 8), (p)[7] = (uint8_t)(V))

void Gf128FromBytes(Gf128 r, const uint8_t p[16])
{
    r[0] = ReverseBits(GETU64(p));
    r[1] = ReverseBits(GETU64(p + 8));
}

void Gf128ToBytes(const Gf128 a, uint8_t p[16])
{
    PUTU64(p, ReverseBits(a[0]));
    PUTU64(p + 8, ReverseBits(a[1]));
}

void Gf128MulBy2(Gf128 r, const Gf128 a)
{
    const uint64_t mask = (uint64_t)1 << 63;

    if (a[1] & mask) {
        r[1] = a[1] << 1 | a[0] >> 63;
        r[0] = a[0] << 1;
        r[0] ^= 0x87;
    } else {
        r[1] = a[1] << 1 | a[0] >> 63;
        r[0] = a[0] << 1;
    }
}

void KAE_Mode_Free(KAE_Mode_Ctx *ctx)
{
    if (ctx == NULL) {
        return;
    }

    if (ctx->cipherCtx) {
        wcrypto_del_cipher_ctx(ctx->cipherCtx);
        ctx->cipherCtx = NULL;
    }

    if (ctx->pool && ctx->opData.in) {
        ctx->setup.br.free(ctx->setup.br.usr, ctx->opData.in);
        ctx->opData.in = NULL;
    }

    if (ctx->pool && ctx->opData.out) {
        ctx->setup.br.free(ctx->setup.br.usr, ctx->opData.out);
        ctx->opData.out = NULL;
    }

    if (ctx->pool && ctx->opData.iv) {
        ctx->setup.br.free(ctx->setup.br.usr, ctx->opData.iv);
        ctx->opData.iv = NULL;
    }

    KaePutNodeToPool(gModesNodePool, ctx->entry);

    (void)memset_s(ctx->key, CIPHER_KEY_SIZE, 0, CIPHER_KEY_SIZE);

    if (ctx->xtsEcb) {
        CRYPT_EAL_CipherFreeCtx(ctx->xtsEcb->ecbCtx);
        if (ctx->xtsEcb->key) {
            (void)memset_s(ctx->xtsEcb->key, ctx->keySize, 0, ctx->keySize);
            free(ctx->xtsEcb->key);
            ctx->xtsEcb->key = NULL;
        }
        if (ctx->xtsEcb->iv) {
            free(ctx->xtsEcb->iv);
            ctx->xtsEcb->iv = NULL;
        }
        free(ctx->xtsEcb);
        ctx->xtsEcb = NULL;
    }
    free(ctx);
    ctx = NULL;
}

KAE_Mode_Ctx *KAE_Mode_NewCtx(int32_t algId)
{
    KAE_Mode_Ctx *ctx = (KAE_Mode_Ctx *)malloc(sizeof(KAE_Mode_Ctx));
    if (ctx == NULL) {
        BSL_ERR_PUSH_ERROR(CRYPT_MEM_ALLOC_FAIL);
        return NULL;
    }
    (void)memset_s(ctx, sizeof(KAE_Mode_Ctx), 0, sizeof(KAE_Mode_Ctx));

    ctx->entry = KaeGetNodeFromPool(gModesNodePool);
    if (ctx->entry == NULL) {
        KAE_Mode_Free(ctx);
        BSL_ERR_PUSH_ERROR(CRYPT_MEM_ALLOC_FAIL);
        return NULL;
    }

    ctx->queue = ctx->entry->queue;
    ctx->pool = ctx->entry->mempool;

    switch (algId) {
        case CRYPT_CIPHER_SM4_ECB:
        case CRYPT_CIPHER_SM4_CBC:
        case CRYPT_CIPHER_SM4_CTR:
        case CRYPT_CIPHER_SM4_XTS:
        case CRYPT_CIPHER_SM4_OFB: {
            ctx->setup.alg = WCRYPTO_CIPHER_SM4;
            ctx->keySize = SM4_KEY_SIZE;
            ctx->cipherSize = SM4_BLOCK_SIZE;
            break;
        }

        case CRYPT_CIPHER_AES128_ECB:
        case CRYPT_CIPHER_AES128_CBC:
        case CRYPT_CIPHER_AES128_CTR:
        case CRYPT_CIPHER_AES128_OFB: {
            ctx->setup.alg = WCRYPTO_CIPHER_AES;
            ctx->keySize = AES128_KEY_SIZE;
            ctx->cipherSize = AES128_BLOCK_SIZE;
            break;
        }

        case CRYPT_CIPHER_AES192_ECB:
        case CRYPT_CIPHER_AES192_CBC:
        case CRYPT_CIPHER_AES192_CTR:
        case CRYPT_CIPHER_AES192_OFB: {
            ctx->setup.alg = WCRYPTO_CIPHER_AES;
            ctx->keySize = AES192_KEY_SIZE;
            ctx->cipherSize = AES192_BLOCK_SIZE;
            break;
        }

        case CRYPT_CIPHER_AES256_ECB:
        case CRYPT_CIPHER_AES256_CBC:
        case CRYPT_CIPHER_AES256_CTR:
        case CRYPT_CIPHER_AES256_OFB: {
            ctx->setup.alg = WCRYPTO_CIPHER_AES;
            ctx->keySize = AES256_KEY_SIZE;
            ctx->cipherSize = AES256_BLOCK_SIZE;
            break;
        }

        default: {
            BSL_ERR_PUSH_ERROR(KAE_WD_NOT_SUPPORT);
            goto ERR;
        }
    }

    switch (algId) {
        case CRYPT_CIPHER_SM4_ECB:
        case CRYPT_CIPHER_AES128_ECB:
        case CRYPT_CIPHER_AES192_ECB:
        case CRYPT_CIPHER_AES256_ECB: {
            ctx->setup.mode = WCRYPTO_CIPHER_ECB;
            break;
        }

        case CRYPT_CIPHER_SM4_CBC:
        case CRYPT_CIPHER_AES128_CBC:
        case CRYPT_CIPHER_AES192_CBC:
        case CRYPT_CIPHER_AES256_CBC: {
            ctx->setup.mode = WCRYPTO_CIPHER_CBC;
            break;
        }

        case CRYPT_CIPHER_SM4_CTR:
        case CRYPT_CIPHER_AES128_CTR:
        case CRYPT_CIPHER_AES192_CTR:
        case CRYPT_CIPHER_AES256_CTR: {
            ctx->setup.mode = WCRYPTO_CIPHER_CTR;
            break;
        }

        case CRYPT_CIPHER_SM4_OFB:
        case CRYPT_CIPHER_AES128_OFB:
        case CRYPT_CIPHER_AES192_OFB:
        case CRYPT_CIPHER_AES256_OFB: {
            ctx->setup.mode = WCRYPTO_CIPHER_OFB;
            break;
        }

        case CRYPT_CIPHER_SM4_XTS: {
            ctx->setup.mode = WCRYPTO_CIPHER_XTS;
            break;
        }

        default: {
            BSL_ERR_PUSH_ERROR(KAE_WD_NOT_SUPPORT);
            goto ERR;
        }
    }
    ctx->algId = algId;
    ctx->setup.br.alloc = (void *)wd_alloc_blk;
    ctx->setup.br.free = (void *)wd_free_blk;
    ctx->setup.br.iova_map = (void *)wd_blk_iova_map;
    ctx->setup.br.iova_unmap = (void *)wd_blk_iova_unmap;
    ctx->setup.br.usr = ctx->pool;
    ctx->cipherCtx = wcrypto_create_cipher_ctx(ctx->queue, &ctx->setup);
    if (ctx->cipherCtx == NULL) {
        BSL_ERR_PUSH_ERROR(KAE_WD_CREATE_CTX_FAIL);
        goto ERR;
    }

    ctx->opData.in = wd_alloc_blk(ctx->pool);
    if (ctx->opData.in == NULL) {
        BSL_ERR_PUSH_ERROR(CRYPT_MEM_ALLOC_FAIL);
        goto ERR;
    }

    ctx->opData.out = wd_alloc_blk(ctx->pool);
    if (ctx->opData.out == NULL) {
        BSL_ERR_PUSH_ERROR(CRYPT_MEM_ALLOC_FAIL);
        goto ERR;
    }

    ctx->pad = CRYPT_PADDING_NONE;

    return ctx;
ERR:
    KAE_Mode_Free(ctx);
    return NULL;
}

static int32_t ECB_Encrypt(KAE_Mode_Ctx *ctx, const uint8_t *in, uint8_t *out, uint32_t *len)
{
    int32_t ret;

    if (*len != ctx->cipherSize) {
        BSL_ERR_PUSH_ERROR(CRYPT_INVALID_ARG);
        return CRYPT_INVALID_ARG;
    }

    XTS_ECB *xtsEcb = ctx->xtsEcb;
    if (ctx->xtsEcb->ecbCtx == NULL) {
        BSL_ERR_PUSH_ERROR(CRYPT_MEM_ALLOC_FAIL);
        return CRYPT_MEM_ALLOC_FAIL;
    }

    ret = CRYPT_EAL_CipherInit(xtsEcb->ecbCtx, xtsEcb->key, xtsEcb->keyLen, NULL, 0, true);
    if (ret != CRYPT_SUCCESS) {
        BSL_ERR_PUSH_ERROR(ret);
        goto ERR;
    }

    ret = CRYPT_EAL_CipherUpdate(xtsEcb->ecbCtx, in, *len, out, len);
    if (ret != CRYPT_SUCCESS) {
        BSL_ERR_PUSH_ERROR(ret);
        goto ERR;
    }
ERR:
    return ret;
}

static int32_t ECB_Decrypt(KAE_Mode_Ctx *ctx, const uint8_t *in, uint8_t *out, uint32_t *len)
{
    int32_t ret;

    if (*len != ctx->cipherSize) {
        BSL_ERR_PUSH_ERROR(CRYPT_INVALID_ARG);
        return CRYPT_INVALID_ARG;
    }

    XTS_ECB *xtsEcb = ctx->xtsEcb;
    if (ctx->xtsEcb->ecbCtx == NULL) {
        BSL_ERR_PUSH_ERROR(CRYPT_MEM_ALLOC_FAIL);
        return CRYPT_MEM_ALLOC_FAIL;
    }

    ret = CRYPT_EAL_CipherInit(xtsEcb->ecbCtx, xtsEcb->key, xtsEcb->keyLen, NULL, 0, false);
    if (ret != CRYPT_SUCCESS) {
        BSL_ERR_PUSH_ERROR(ret);
        goto ERR;
    }

    ret = CRYPT_EAL_CipherUpdate(xtsEcb->ecbCtx, in, *len, out, len);
    if (ret != CRYPT_SUCCESS) {
        BSL_ERR_PUSH_ERROR(ret);
        goto ERR;
    }
ERR:
    return ret;
}

int32_t KAE_Mode_InitCtx(KAE_Mode_Ctx *ctx, const uint8_t *key, uint32_t keyLen, const uint8_t *iv, uint32_t ivLen,
                         const BSL_Param *param, bool enc)
{
    if (ctx == NULL || ctx->cipherCtx == NULL || ctx->pool == NULL || key == NULL) {
        BSL_ERR_PUSH_ERROR(CRYPT_NULL_INPUT);
        return CRYPT_NULL_INPUT;
    }

    (void)param;

    int32_t ret;

    switch (ctx->setup.mode) {
        case WCRYPTO_CIPHER_XTS: {
            if (keyLen != 2 * ctx->keySize) {
                BSL_ERR_PUSH_ERROR(CRYPT_INVALID_ARG);
                return CRYPT_INVALID_ARG;
            }

            if (memcmp(key + (keyLen >> 1), key, keyLen >> 1) == 0) {
                BSL_ERR_PUSH_ERROR(CRYPT_MODES_ERR_KEY);
                return CRYPT_MODES_ERR_KEY;
            }

            break;
        }
        default: {
            if (keyLen != ctx->keySize) {
                BSL_ERR_PUSH_ERROR(CRYPT_INVALID_ARG);
                return CRYPT_INVALID_ARG;
            }
            break;
        }
    }
    (void)memcpy_s(ctx->key, CIPHER_KEY_SIZE, key, keyLen);
    ret = wcrypto_set_cipher_key(ctx->cipherCtx, (uint8_t *)key, keyLen);
    if (ret != CRYPT_SUCCESS) {
        BSL_ERR_PUSH_ERROR(KAE_WD_DO_CIPHER_FAIL);
        return KAE_WD_DO_CIPHER_FAIL;
    }

    switch (ctx->setup.mode) {
        case WCRYPTO_CIPHER_CBC:
        case WCRYPTO_CIPHER_CTR:
        case WCRYPTO_CIPHER_OFB:
        case WCRYPTO_CIPHER_XTS: {
            if (iv == NULL) {
                ret = CRYPT_NULL_INPUT;
                BSL_ERR_PUSH_ERROR(CRYPT_NULL_INPUT);
                goto ERR;
            }

            if (ivLen != ctx->cipherSize) {
                ret = CRYPT_MODES_IVLEN_ERROR;
                BSL_ERR_PUSH_ERROR(CRYPT_MODES_IVLEN_ERROR);
                goto ERR;
            }
        }
        default:
            break;
    }

    ctx->opData.iv = wd_alloc_blk(ctx->pool);
    if (ctx->opData.iv == NULL) {
        ret = CRYPT_MEM_ALLOC_FAIL;
        BSL_ERR_PUSH_ERROR(CRYPT_MEM_ALLOC_FAIL);
        goto ERR;
    }

    (void)memcpy_s(ctx->opData.iv, ivLen, iv, ivLen);
    ctx->opData.iv_bytes = ivLen;

    if (ctx->setup.mode == WCRYPTO_CIPHER_CBC || ctx->setup.mode == WCRYPTO_CIPHER_OFB) {
        (void)memcpy_s(ctx->opData.out, ivLen, iv, ivLen);
        ctx->opData.out_bytes = ivLen;
    }

    ctx->opData.op_type = enc ? WCRYPTO_CIPHER_ENCRYPTION : WCRYPTO_CIPHER_DECRYPTION;

    if (ctx->setup.mode == WCRYPTO_CIPHER_XTS) {
        ctx->xtsEcb = (XTS_ECB *)malloc(sizeof(XTS_ECB));
        if (ctx->setup.alg == WCRYPTO_CIPHER_SM4) {
            ctx->xtsEcb->ecbCtx = CRYPT_EAL_CipherNewCtx(CRYPT_CIPHER_SM4_ECB);
        }
        ctx->xtsEcb->iv = (uint8_t *)malloc(ctx->cipherSize);
        (void)memcpy_s(ctx->xtsEcb->iv, ctx->cipherSize, iv, ctx->cipherSize);
        ctx->xtsEcb->key = (uint8_t *)malloc(ctx->keySize);
        (void)memcpy_s(ctx->xtsEcb->key, ctx->keySize, key + ctx->keySize, ctx->keySize);
        ctx->xtsEcb->keyLen = ctx->keySize;
        uint32_t outLen = ctx->cipherSize;
        ret = ECB_Encrypt(ctx, ctx->xtsEcb->iv, ctx->xtsEcb->iv, &outLen);
        if (ret != CRYPT_SUCCESS) {
            BSL_ERR_PUSH_ERROR(ret);
            goto ERR;
        }
    }

    return CRYPT_SUCCESS;
ERR:
    return ret;
}

int32_t KAE_Mode_XTS_Update(KAE_Mode_Ctx *ctx, const uint8_t *in, uint32_t inLen, uint8_t *out, uint32_t *outLen)
{
    if (inLen < ctx->cipherSize) {
        BSL_ERR_PUSH_ERROR(CRYPT_INVALID_ARG);
        return CRYPT_INVALID_ARG;
    }

    size_t nblocks = inLen / ctx->cipherSize + 1;
    *outLen = 0;

    for (size_t i = 0; i < nblocks - 2; ++i) {
        ctx->opData.in_bytes = ctx->cipherSize;
        (void)memcpy_s(ctx->opData.in, ctx->cipherSize, in, ctx->opData.in_bytes);
        if (wcrypto_do_cipher(ctx->cipherCtx, &ctx->opData, NULL) != CRYPT_SUCCESS) {
            BSL_ERR_PUSH_ERROR(KAE_WD_DO_CIPHER_FAIL);
            return KAE_WD_DO_CIPHER_FAIL;
        }
        Gf128 a;
        Gf128FromBytes(a, ctx->xtsEcb->iv);
        Gf128MulBy2(a, a);
        Gf128ToBytes(a, ctx->xtsEcb->iv);
        ctx->opData.iv_bytes = ctx->cipherSize;
        (void)ECB_Decrypt(ctx, ctx->xtsEcb->iv, ctx->opData.iv, &ctx->opData.iv_bytes);
        (void)memcpy_s(out, ctx->cipherSize, ctx->opData.out, ctx->cipherSize);
        in += ctx->cipherSize;
        inLen -= ctx->cipherSize;
        out += ctx->cipherSize;
        outLen += ctx->cipherSize;
    }

    if (inLen % ctx->cipherSize == 0) {
        ctx->opData.in_bytes = ctx->cipherSize;
        (void)memcpy_s(ctx->opData.in, ctx->cipherSize, in, ctx->opData.in_bytes);
        if (wcrypto_do_cipher(ctx->cipherCtx, &ctx->opData, NULL) != CRYPT_SUCCESS) {
            BSL_ERR_PUSH_ERROR(KAE_WD_DO_CIPHER_FAIL);
            return KAE_WD_DO_CIPHER_FAIL;
        }
        Gf128 a;
        Gf128FromBytes(a, ctx->xtsEcb->iv);
        Gf128MulBy2(a, a);
        Gf128ToBytes(a, ctx->xtsEcb->iv);
        ctx->opData.iv_bytes = ctx->cipherSize;
        (void)ECB_Decrypt(ctx, ctx->xtsEcb->iv, ctx->opData.iv, &ctx->opData.iv_bytes);
        (void)memcpy_s(out, ctx->cipherSize, ctx->opData.out, ctx->cipherSize);
        *outLen += ctx->cipherSize;
    } else {
        ctx->opData.in_bytes = ctx->cipherSize;
        (void)memcpy_s(ctx->opData.in, ctx->cipherSize, in, ctx->opData.in_bytes);
        if (wcrypto_do_cipher(ctx->cipherCtx, &ctx->opData, NULL) != CRYPT_SUCCESS) {
            BSL_ERR_PUSH_ERROR(KAE_WD_DO_CIPHER_FAIL);
            return KAE_WD_DO_CIPHER_FAIL;
        }
        Gf128 a;
        Gf128FromBytes(a, ctx->xtsEcb->iv);
        Gf128MulBy2(a, a);
        Gf128ToBytes(a, ctx->xtsEcb->iv);
        ctx->opData.iv_bytes = ctx->cipherSize;
        (void)ECB_Decrypt(ctx, ctx->xtsEcb->iv, ctx->opData.iv, &ctx->opData.iv_bytes);

        in += 16;
        inLen -= 16;

        (void)memcpy_s(out + 16, ctx->cipherSize, ctx->opData.out, inLen);
        ctx->opData.in_bytes = ctx->cipherSize;
        (void)memcpy_s(ctx->opData.in, ctx->cipherSize, in, inLen);
        if (wcrypto_do_cipher(ctx->cipherCtx, &ctx->opData, NULL) != CRYPT_SUCCESS) {
            BSL_ERR_PUSH_ERROR(KAE_WD_DO_CIPHER_FAIL);
            return KAE_WD_DO_CIPHER_FAIL;
        }
        (void)memcpy_s(out, ctx->cipherSize, ctx->opData.out, ctx->cipherSize);
        *outLen += (ctx->cipherSize + inLen);
    }
    return CRYPT_SUCCESS;
}

int32_t WD_DoCipherBlockImpl(KAE_Mode_Ctx *ctx, const uint8_t *in)
{
    if (wcrypto_do_cipher(ctx->cipherCtx, &ctx->opData, NULL) != CRYPT_SUCCESS) {
        BSL_ERR_PUSH_ERROR(KAE_WD_DO_CIPHER_FAIL);
        return KAE_WD_DO_CIPHER_FAIL;
    }
    switch (ctx->setup.mode) {
        case WCRYPTO_CIPHER_CBC: {
            if (ctx->opData.op_type == WCRYPTO_CIPHER_ENCRYPTION) {
                (void)memcpy_s(ctx->opData.iv, ctx->cipherSize, ctx->opData.out, ctx->cipherSize);
            } else {
                (void)memcpy_s(ctx->opData.iv, ctx->cipherSize, in, ctx->cipherSize);
            }
            break;
        }
        default:
            break;
    }
    return CRYPT_SUCCESS;
}

bool KAE_UpdateCompare(uint32_t l, uint32_t r, KAE_Modes_UpdateType type)
{
    return (type == NO_BLK_LEFT) ? (l >= r) : (l > r);
}

int32_t KAE_Mode_UpdateCore(KAE_Mode_Ctx *ctx, const uint8_t *in, uint32_t inLen, uint8_t *out, uint32_t *outLen,
                            KAE_Modes_UpdateType type)
{
    int32_t ret;
    int cnt = 0;

    if (ctx->remain > 0) {
        if (KAE_UpdateCompare(inLen + ctx->remain, ctx->cipherSize, type)) {
            (void)memcpy_s(ctx->block + ctx->remain, CIPHER_CACHE_SIZE - ctx->remain, in,
                           ctx->cipherSize - ctx->remain);
            in += (ctx->cipherSize - ctx->remain);
            inLen -= (ctx->cipherSize - ctx->remain);
            ctx->remain = 0;
            ctx->opData.in_bytes = ctx->cipherSize;
            (void)memcpy_s(ctx->opData.in, ctx->cipherSize, ctx->block, ctx->opData.in_bytes);
            ret = WD_DoCipherBlockImpl(ctx, ctx->block);
            if (ret != CRYPT_SUCCESS) {
                BSL_ERR_PUSH_ERROR(ret);
                return ret;
            }
            (void)memcpy_s(out, ctx->cipherSize, ctx->opData.out, ctx->cipherSize);
            out += ctx->cipherSize;
            (*outLen) += ctx->cipherSize;
        } else {
            (void)memcpy_s(ctx->block + ctx->remain, CIPHER_CACHE_SIZE - ctx->remain, in, inLen);
            ctx->remain += inLen;
            return CRYPT_SUCCESS;
        }
    }

    while (KAE_UpdateCompare(inLen, cnt + ctx->cipherSize, type)) {
        ctx->opData.in_bytes = ctx->cipherSize;
        (void)memcpy_s(ctx->opData.in, ctx->cipherSize, in, ctx->opData.in_bytes);
        ret = WD_DoCipherBlockImpl(ctx, in);
        if (ret != CRYPT_SUCCESS) {
            BSL_ERR_PUSH_ERROR(ret);
            return ret;
        }
        (void)memcpy_s(out, ctx->cipherSize, ctx->opData.out, ctx->cipherSize);
        out += ctx->cipherSize;
        (*outLen) += ctx->cipherSize;
        in += ctx->cipherSize;
        cnt += ctx->cipherSize;
    }

    ctx->remain = inLen - cnt;
    (void)memcpy_s(ctx->block, CIPHER_CACHE_SIZE, in, ctx->remain);

    return CRYPT_SUCCESS;
}

int32_t KAE_Mode_Update(KAE_Mode_Ctx *ctx, const uint8_t *in, uint32_t inLen, uint8_t *out, uint32_t *outLen)
{
    if ((ctx == NULL) || (ctx->cipherCtx == NULL) || (in == NULL && inLen != 0) || out == NULL || outLen == NULL) {
        BSL_ERR_PUSH_ERROR(CRYPT_NULL_INPUT);
        return CRYPT_NULL_INPUT;
    }

    (*outLen) = 0;

    if (inLen == 0) {
        return CRYPT_SUCCESS;
    }

    if (ctx->setup.mode == WCRYPTO_CIPHER_XTS) {
        return KAE_Mode_XTS_Update(ctx, in, inLen, out, outLen);
    }

    if (ctx->opData.op_type == WCRYPTO_CIPHER_ENCRYPTION || ctx->pad == CRYPT_PADDING_NONE) {
        return KAE_Mode_UpdateCore(ctx, in, inLen, out, outLen, NO_BLK_LEFT);
    } else {
        return KAE_Mode_UpdateCore(ctx, in, inLen, out, outLen, BLK_LEFT);
    }
}

static bool IfXts(CRYPT_CIPHER_AlgId id)
{
    CRYPT_CIPHER_AlgId XTS_list[] = {
        CRYPT_CIPHER_SM4_XTS,
    };
    for (uint32_t i = 0; i < sizeof(XTS_list) / sizeof(XTS_list[0]); i++) {
        if (id == XTS_list[i]) {
            return true;
        }
    }
    return false;
}

int32_t UnPaddingISO7816(const uint8_t *pad, uint32_t padLen, uint32_t *finLen)
{
    uint32_t len;
    const uint8_t *p = pad;
    len = padLen - 1;
    while (*(p + len) == 0 && len > 0) {
        len--;
    }
    len = (*(p + len) == 0x80) ? len : padLen;

    if (len == padLen) {
        BSL_ERR_PUSH_ERROR(CRYPT_EAL_CIPHER_DATA_ERROR);
        return CRYPT_EAL_CIPHER_DATA_ERROR;
    }
    (*finLen) = len;
    return CRYPT_SUCCESS;
}

int32_t UnPaddingX923(const uint8_t *pad, uint32_t padLen, uint32_t *finLen)
{
    uint32_t len, pos, i;
    uint32_t check = 0;
    len = pad[padLen - 1];

    check |= (uint32_t)(len > padLen);

    pos = padLen - len;
    for (i = 0; i < padLen - 1; i++) {
        check |= (pad[i] * (uint32_t)(i >= pos));
    }

    if (check != 0) {
        BSL_ERR_PUSH_ERROR(CRYPT_EAL_CIPHER_DATA_ERROR);
        return CRYPT_EAL_CIPHER_DATA_ERROR;
    }

    (*finLen) = padLen - len;
    return CRYPT_SUCCESS;
}

int32_t UnPaddingPkcs(const uint8_t *pad, uint32_t padLen, uint32_t *finLen)
{
    uint32_t len, pos, i;
    uint32_t check = 0;

    len = pad[padLen - 1];
    check |= (uint32_t)((len == 0) || (len > padLen));

    pos = padLen - len;
    for (i = 0; i < padLen; i++) {
        check |= ((pad[i] ^ len) * (uint32_t)(i >= pos));
    }

    if (check != 0) {
        BSL_ERR_PUSH_ERROR(CRYPT_EAL_CIPHER_DATA_ERROR);
        return CRYPT_EAL_CIPHER_DATA_ERROR;
    }

    (*finLen) = padLen - len;
    return CRYPT_SUCCESS;
}

int32_t Mode_BlockUnPadding(int32_t padding, const uint8_t *pad, uint32_t padLen, uint32_t *dataLen)
{
    int32_t ret = 0;
    uint32_t len = *dataLen;
    switch (padding) {
        case CRYPT_PADDING_ISO7816:
            ret = UnPaddingISO7816(pad, padLen, &len);
            break;
        case CRYPT_PADDING_X923:
            ret = UnPaddingX923(pad, padLen, &len);
            break;
        case CRYPT_PADDING_PKCS5:
        case CRYPT_PADDING_PKCS7:
            ret = UnPaddingPkcs(pad, padLen, &len);
            break;
        default:
            break;
    }

    *dataLen = len;
    return ret;
}

static int32_t MODES_BlockPadding(int32_t algId, int32_t padding, uint8_t blockSize, uint8_t *data, uint8_t *dataLen)
{
    uint8_t tempLen = *dataLen;
    uint8_t *pad = data + tempLen;
    uint8_t padLen = blockSize - tempLen;
    uint8_t i;
    *dataLen += padLen;
    switch (padding) {
        case CRYPT_PADDING_NONE:
            *dataLen = tempLen;
            if (tempLen % blockSize != 0) {
                return IfXts(algId) ? CRYPT_SUCCESS : CRYPT_MODE_ERR_INPUT_LEN;
            }
            break;
        case CRYPT_PADDING_ZEROS:
            for (i = 0; i < padLen; i++) {
                pad[i] = 0x00L;
            }
            break;
        case CRYPT_PADDING_ISO7816:
            pad[0] = 0x80;
            for (i = 1; i < padLen; i++) {
                pad[i] = 0x00L;
            }
            break;
        case CRYPT_PADDING_X923:
            for (i = 0; i < padLen - 1; i++) {
                pad[i] = 0x00L;
            }
            pad[padLen - 1] = padLen;
            break;
        case CRYPT_PADDING_PKCS5:
        case CRYPT_PADDING_PKCS7:
            for (i = 0; i < padLen; i++) {
                pad[i] = padLen;
            }
            break;
        default:
            *dataLen = tempLen;
            return CRYPT_INVALID_ARG;
    }
    return CRYPT_SUCCESS;
}

int32_t KAE_Mode_Final_PAD(KAE_Mode_Ctx *ctx, uint8_t *out, uint32_t *outLen)
{
    int32_t ret;
    uint32_t paddingSize = 0;

    if (ctx->remain == 0 && ctx->pad == CRYPT_PADDING_NONE) {
        *outLen = 0;
        return CRYPT_SUCCESS;
    }

    if (ctx->opData.op_type == WCRYPTO_CIPHER_ENCRYPTION) {
        uint32_t blockN = ctx->remain / ctx->cipherSize;
        uint32_t p = blockN * ctx->cipherSize;
        uint8_t len = ctx->remain - p;
        ret = MODES_BlockPadding(ctx->algId, ctx->pad, ctx->cipherSize, ctx->block + p, &len);
        if (ret != CRYPT_SUCCESS) {
            BSL_ERR_PUSH_ERROR(ret);
            return ret;
        }
        ctx->remain = p + len;
    }

    ctx->opData.in_bytes = ctx->remain;

    if (*outLen < ctx->opData.in_bytes) {
        BSL_ERR_PUSH_ERROR(CRYPT_MODE_BUFF_LEN_NOT_ENOUGH);
        return CRYPT_MODE_BUFF_LEN_NOT_ENOUGH;
    }

    (void)memcpy_s(ctx->opData.in, CIPHER_CACHE_SIZE, ctx->block, ctx->opData.in_bytes);
    if (wcrypto_do_cipher(ctx->cipherCtx, &ctx->opData, NULL) != CRYPT_SUCCESS) {
        BSL_ERR_PUSH_ERROR(KAE_WD_DO_CIPHER_FAIL);
        return KAE_WD_DO_CIPHER_FAIL;
    }

    if (ctx->opData.op_type == WCRYPTO_CIPHER_ENCRYPTION) {
        (void)memcpy_s(out, CIPHER_CACHE_SIZE, ctx->opData.out, ctx->opData.in_bytes);
        (*outLen) = ctx->opData.in_bytes;
    }

    if (ctx->opData.op_type == WCRYPTO_CIPHER_DECRYPTION) {
        (void)memcpy_s(out, CIPHER_CACHE_SIZE, ctx->opData.out, ctx->opData.in_bytes);
        ret = Mode_BlockUnPadding(ctx->pad, out, ctx->opData.in_bytes, outLen);
        if (ret != CRYPT_SUCCESS) {
            BSL_ERR_PUSH_ERROR(ret);
            return ret;
        }
    }

    return CRYPT_SUCCESS;
}

int32_t KAE_Mode_Final_NO_PAD(KAE_Mode_Ctx *ctx, uint8_t *out, uint32_t *outLen)
{
    if (ctx->remain == 0) {
        *outLen = ctx->remain;
        return CRYPT_SUCCESS;
    }

    ctx->opData.in_bytes = ctx->remain;

    if (*outLen < ctx->opData.in_bytes) {
        BSL_ERR_PUSH_ERROR(CRYPT_MODE_BUFF_LEN_NOT_ENOUGH);
        return CRYPT_MODE_BUFF_LEN_NOT_ENOUGH;
    }

    memcpy(ctx->opData.in, ctx->block, ctx->opData.in_bytes);
    if (wcrypto_do_cipher(ctx->cipherCtx, &ctx->opData, NULL) != CRYPT_SUCCESS) {
        BSL_ERR_PUSH_ERROR(KAE_WD_DO_CIPHER_FAIL);
        return KAE_WD_DO_CIPHER_FAIL;
    }
    memcpy(out, ctx->opData.out, ctx->opData.in_bytes);
    (*outLen) = ctx->opData.in_bytes;
    return CRYPT_SUCCESS;
}

int32_t KAE_Mode_Final(KAE_Mode_Ctx *ctx, uint8_t *out, uint32_t *outLen)
{
    if ((ctx == NULL) || (ctx->cipherCtx == NULL) || (out == NULL && outLen != 0)) {
        BSL_ERR_PUSH_ERROR(CRYPT_NULL_INPUT);
        return CRYPT_NULL_INPUT;
    }

    if (outLen == 0) {
        return CRYPT_SUCCESS;
    }

    if (ctx->setup.mode == WCRYPTO_CIPHER_CTR || ctx->setup.mode == WCRYPTO_CIPHER_OFB) {
        return KAE_Mode_Final_NO_PAD(ctx, out, outLen);
    } else {
        return KAE_Mode_Final_PAD(ctx, out, outLen);
    }
}

static int32_t MODES_SetPaddingCheck(int32_t pad)
{
    if (pad >= CRYPT_PADDING_MAX_COUNT || pad < CRYPT_PADDING_NONE) {
        BSL_ERR_PUSH_ERROR(CRYPT_MODES_PADDING_NOT_SUPPORT);
        return CRYPT_MODES_PADDING_NOT_SUPPORT;
    }
    return CRYPT_SUCCESS;
}

int32_t KAE_Mode_SetIv(KAE_Mode_Ctx *ctx, void *val, uint32_t valLen)
{
    if (val == NULL) {
        BSL_ERR_PUSH_ERROR(CRYPT_NULL_INPUT);
        return CRYPT_NULL_INPUT;
    }

    if (valLen != ctx->cipherSize) {
        BSL_ERR_PUSH_ERROR(CRYPT_MODES_IVLEN_ERROR);
        return CRYPT_MODES_IVLEN_ERROR;
    }

    if (memcpy_s(ctx->opData.iv, valLen, val, valLen) != EOK) {
        BSL_ERR_PUSH_ERROR(CRYPT_SECUREC_FAIL);
        return CRYPT_SECUREC_FAIL;
    }
    ctx->opData.iv_bytes = valLen;

    if (memcpy_s(ctx->opData.out, valLen, val, valLen) != EOK) {
        BSL_ERR_PUSH_ERROR(CRYPT_SECUREC_FAIL);
        return CRYPT_SECUREC_FAIL;
    }
    ctx->opData.out_bytes = valLen;

    if (ctx->setup.mode == WCRYPTO_CIPHER_XTS) {
        (void)memcpy_s(ctx->xtsEcb->iv, ctx->cipherSize, val, valLen);
        uint32_t outLen = ctx->cipherSize;
        int32_t ret = ECB_Encrypt(ctx, ctx->xtsEcb->iv, ctx->xtsEcb->iv, &outLen);
        if (ret != CRYPT_SUCCESS) {
            BSL_ERR_PUSH_ERROR(ret);
            return ret;
        }
    }
    return CRYPT_SUCCESS;
}

int32_t KAE_Mode_GetIv(KAE_Mode_Ctx *ctx, void *val, uint32_t valLen)
{
    if (val == NULL) {
        BSL_ERR_PUSH_ERROR(CRYPT_NULL_INPUT);
        return CRYPT_NULL_INPUT;
    }

    uint32_t ivLen = ctx->cipherSize;

    if (valLen != ivLen) {
        BSL_ERR_PUSH_ERROR(CRYPT_MODE_ERR_INPUT_LEN);
        return CRYPT_MODE_ERR_INPUT_LEN;
    }

    if (memcpy_s(val, valLen, ctx->opData.iv, ivLen) != EOK) {
        BSL_ERR_PUSH_ERROR(CRYPT_SECUREC_FAIL);
        return CRYPT_SECUREC_FAIL;
    }

    return CRYPT_SUCCESS;
}

int32_t KAE_Mode_Deinit(KAE_Mode_Ctx *ctx)
{
    if (ctx == NULL) {
        BSL_ERR_PUSH_ERROR(CRYPT_NULL_INPUT);
        return CRYPT_NULL_INPUT;
    }

    if (ctx->cipherCtx) {
        wcrypto_del_cipher_ctx(ctx->cipherCtx);
        ctx->cipherCtx = NULL;
    }

    ctx->cipherCtx = wcrypto_create_cipher_ctx(ctx->queue, &ctx->setup);
    if (ctx->cipherCtx == NULL) {
        KAE_Mode_Free(ctx);
        BSL_ERR_PUSH_ERROR(KAE_WD_CREATE_CTX_FAIL);
        return KAE_WD_CREATE_CTX_FAIL;
    }

    if (ctx->pool && ctx->opData.iv) {
        ctx->setup.br.free(ctx->setup.br.usr, ctx->opData.iv);
        ctx->opData.iv = NULL;
    }

    if (ctx->xtsEcb) {
        CRYPT_EAL_CipherFreeCtx(ctx->xtsEcb->ecbCtx);
        if (ctx->xtsEcb->key) {
            (void)memset_s(ctx->xtsEcb->key, ctx->keySize, 0, ctx->keySize);
            free(ctx->xtsEcb->key);
            ctx->xtsEcb->key = NULL;
        }
        if (ctx->xtsEcb->iv) {
            free(ctx->xtsEcb->iv);
            ctx->xtsEcb->iv = NULL;
        }
        free(ctx->xtsEcb);
        ctx->xtsEcb = NULL;
    }

    (void)memset_s(ctx->opData.in, ctx->opData.in_bytes, 0, ctx->opData.in_bytes);
    (void)memset_s(ctx->opData.out, ctx->opData.out_bytes, 0, ctx->opData.out_bytes);
    (void)memset_s(ctx->block, CIPHER_CACHE_SIZE, 0, CIPHER_CACHE_SIZE);
    (void)memset_s(ctx->key, CIPHER_KEY_SIZE, 0, CIPHER_KEY_SIZE);
    ctx->remain = 0;
    return CRYPT_SUCCESS;
}

int32_t KAE_Mode_Reinit(KAE_Mode_Ctx *ctx, void *val, uint32_t valLen)
{
    if (ctx == NULL || val == NULL) {
        BSL_ERR_PUSH_ERROR(CRYPT_NULL_INPUT);
        return CRYPT_NULL_INPUT;
    }

    uint8_t key[CIPHER_KEY_SIZE];
    (void)memcpy_s(key, CIPHER_KEY_SIZE, ctx->key, CIPHER_KEY_SIZE);
    uint8_t keyLen = (ctx->setup.mode == WCRYPTO_CIPHER_XTS) ? 2 * ctx->keySize : ctx->keySize;
    bool enc = (ctx->opData.op_type == WCRYPTO_CIPHER_ENCRYPTION);
    int32_t ret = KAE_Mode_Deinit(ctx);
    if (ret != CRYPT_SUCCESS) {
        BSL_ERR_PUSH_ERROR(ret);
        return ret;
    }
    ret = KAE_Mode_InitCtx(ctx, key, keyLen, (const uint8_t *)val, valLen, NULL, enc);
    if (ret != CRYPT_SUCCESS) {
        BSL_ERR_PUSH_ERROR(ret);
        return ret;
    }
    return CRYPT_SUCCESS;
}

int32_t KAE_Mode_Ctrl(KAE_Mode_Ctx *ctx, int32_t cmd, void *val, uint32_t valLen)
{
    int32_t ret;
    if (ctx == NULL || ctx->cipherCtx == NULL) {
        BSL_ERR_PUSH_ERROR(CRYPT_NULL_INPUT);
        return CRYPT_NULL_INPUT;
    }

    switch (cmd) {
        case CRYPT_CTRL_SET_PADDING:
            ret = MODES_SetPaddingCheck(*(int32_t *)val);
            if (ret != CRYPT_SUCCESS) {
                return ret;
            }
            ctx->pad = *(int32_t *)val;
            return CRYPT_SUCCESS;
        case CRYPT_CTRL_GET_PADDING:
            *(int32_t *)val = ctx->pad;
            return CRYPT_SUCCESS;
        case CRYPT_CTRL_GET_BLOCKSIZE:
            if (val == NULL || valLen != sizeof(uint32_t)) {
                BSL_ERR_PUSH_ERROR(CRYPT_MODE_ERR_INPUT_LEN);
                return CRYPT_MODE_ERR_INPUT_LEN;
            }
            *(int32_t *)val = ctx->cipherSize;
            return CRYPT_SUCCESS;
        case CRYPT_CTRL_REINIT_STATUS:
            return KAE_Mode_Reinit(ctx, val, valLen);
        case CRYPT_CTRL_GET_IV:
            return KAE_Mode_GetIv(ctx, (uint8_t *)val, valLen);
        default:
            BSL_ERR_PUSH_ERROR(CRYPT_MODES_CTRL_TYPE_ERROR);
            return CRYPT_MODES_CTRL_TYPE_ERROR;
    }
}
